/*
linux_lidar.c
offer lidar data from linux board function realize
designed by lunanting

---------------------
2020-12-17 15:05:54
create file
---------------------
*/

#include "linux_lidar.h"
#include "string.h"
#include "../../algorithm/crc_check.h"
#include "../../bsp/cli_cmd.h"
#include "aos/kernel.h"
#include "../../bsp/xp_uart.h"
#include "stdio.h"
#include "../pansonic/wheel.h"
#include "../../1km/agv/lidar_navigation.h"

static int xp_linux_lidar_debug(char *type,char *fun,char *param);
static void xp_linux_lidar_data_thread(void *arg);
static void xp_linux_lidar_blow_thread(void *arg);
__weak Odom_class *xp_odom_object_get(void){
    static Odom_class odom={0};
    return &odom;
}


static LINUX_lidar lidar={0};

const char get_lidar[]="xp get lidar";

/*
init linux lidar module
param:
    port:uart port
    speed:uart speed
return:
    0=success
*/
int xp_linux_lidar_init(int port,int speed){
    int sta=0;

    lidar.uart.port=port;
    lidar.uart.config.baud_rate=speed;
    lidar.uart.config.data_width=DATA_WIDTH_8BIT;
    lidar.uart.config.parity=NO_PARITY;
    lidar.uart.config.stop_bits=STOP_BITS_1;
    lidar.uart.config.flow_control=FLOW_CONTROL_DISABLED;
    lidar.uart.config.mode=MODE_TX_RX;
	lidar.msg.minDis=255;
    xp_linux_lidar_blowIO_set(100);
    aos_mutex_new(&lidar.mux);
    sta=hal_uart_init(&lidar.uart);
    if(sta!=0){
        perr("linux lidar uart init fail~");
        return -1;
    }

    if(aos_task_new("linux lidar",xp_linux_lidar_data_thread,NULL,2048)!=0){
        perr("linux lidar data thread create fail~");
        return -2;
    }
    aos_task_new("lidar blow",xp_linux_lidar_blow_thread,NULL,1500);
    if(xp_cli_debug_add(xp_linux_lidar_debug)!=0)return -1;
    lidar.init=1;
    return 0;
}




/*
get lidar data thread
*/
static void xp_linux_lidar_data_thread(void *arg){
    char buf[sizeof(LIDAR_data)+1];
	u32 rec_len=0;
    int sta=0;
    int flag=0;
    u8 crc=0;
    const int target_len=sizeof(LIDAR_data);
    Odom_class *odom = xp_odom_object_get();

    while(1){
        memset(buf,0,sizeof(buf));
        xp_uart_rec_clear(&lidar.uart);
        // xp_linux_lidar_send(get_lidar,strlen(get_lidar));
        xp_linux_lidar_send_location(lidar.location.x,lidar.location.y,lidar.location.angle);
        // hal_uart_send(&lidar.uart,get_lidar,strlen(get_lidar),5);
        sta=hal_uart_recv_II(&lidar.uart,buf,target_len,&rec_len,200);
        if(sta!=0){
            if(flag<5){
                flag++;
            }
            if(flag==5){
                // flag=6;
                lidar.msg.minDis=0;
                lidar.msg.maxHight=0;
                lidar.msg.data_valid=0;
                println("linux lidar offline~");
                println("===================>linux no response !!!");
                aos_msleep(2000);
            }
            xp_linux_lidar_send("NO\r\n",4);
            aos_msleep(100);
            continue;
        }
        if(flag>=5){
            println("linux lidar online");
        }
        flag=0;
        if(strstr(buf,"xp")!=buf){
            println("not \"xp\" start~ ,string: %s",buf);
            goto END;
        }
        crc=xp_crc8((u8 *)buf+3,target_len-3);
        if(buf[2]==crc){
            memcpy(&lidar.msg,buf,target_len);
            // lidar.msg.minDis=buf[3];
            // lidar.msg.maxHight=buf[4];
            // lidar.msg.lidar_valid=buf[5];
            // lidar.msg.camera_valid=buf[6];
            // lidar.msg.ss_valid=buf[7];
            // lidar.msg.data_valid=buf[8];
            // println("linux lidar msg: [%d,%d,%d,%d,%d,%d,%d]",buf[3],buf[4],buf[5],buf[6],buf[7],buf[8],buf[9]);
        }
        else{
            println("crc fail~ buf[2]=%d,crc=%d",buf[2],crc);
        }
        END:
        aos_msleep(50);
    }
}


/*
send data to linux
param:
    data:send data
    len:data lenght
return:
    0=success
*/
int xp_linux_lidar_send(const char *data,int len){
    int sta=0;
    if(aos_mutex_lock(&lidar.mux,100)!=0){
        return -1;
    }
    sta=hal_uart_send(&lidar.uart,data,len,10);
    aos_mutex_unlock(&lidar.mux);
    return sta;
}

/*
send location message
param:
    x:x encode
    y:y encode
    angle: angle
return:
    0=success
*/
int xp_linux_lidar_send_location(int x,int y,float angle){
    char buf[100];

    sprintf(buf,"location:(%d,%d,%0.2f);do=%d;error=%d",x,y,angle,lidar.do_angle,lidar.error);
    return xp_linux_lidar_send(buf,strlen(buf));
}


/*
set location
param:
    x:x value
    y:y value
    angle: angle *1000
*/
void xp_linux_lidar_location_set(int x,int y,float angle){
    lidar.location.x=x;
    lidar.location.y=y;
    lidar.location.angle=angle;
}

/************************public functio***********************/
/*
get lidar min dis
return: dis ,255=not valid cm
*/
int xp_linux_lidar_dis_get(void){
    if(lidar.msg.data_valid==1){
        if((xp_linux_lidar_doAngle_get() == 0) && (lidar.msg.minDis == 0)){
            return 255;                             //在正常测距模式测到距离0，则返回错误值255
        }
        else{
            return lidar.msg.minDis;
        }
    }
    else{
        return 255;
    }
}
int xp_linux_lidar_dis2_get(void){
    if(lidar.msg.data_valid==1){
        if((xp_linux_lidar_doAngle_get() == 0) && (lidar.msg.minDis == 0)){
            return 255;                             //在正常测距模式测到距离0，则返回错误值255
        }
        else{
            return lidar.msg.minDis2;
        }
    }
    else{
        return 255;
    }
}
int xp_linux_lidar_dis3_get(void){
    if(lidar.msg.data_valid==1){
        return lidar.msg.minDis3;
        //return 160;
    }
    else{
        return 255;
    }
}

/*
get lidar max hight
return: hight ,255=not valid cm
*/
int xp_linux_lidar_hight_get(void){
    if(lidar.msg.data_valid==1){
        return lidar.msg.maxHight;
    }
    else{
        return 255;
    }
}

/*
get lidar valid status
return: valid status,0=not avalible.1=normal
*/
int xp_linux_lidar_valid_get(void){
    return lidar.msg.data_valid;
}


/*
get lidar msg
return: lidar_data
*/
LIDAR_data *xp_linux_lidar_data_get(void){
    return &lidar.msg;
}


/*
get corner status
return: corner status 1=corner,0=normal
*/
int xp_linux_lidar_getCorner(void){
    return lidar.msg.corner;
}

/*
get do angle state
*/
int xp_linux_lidar_doAngle_get(void){
    return lidar.do_angle;
}

/*
set do angle state
param:
    state:do angle state
*/
void xp_linux_lidar_doAngle_set(int state){
    if(state < 0 || state >= LIDAR_MODE_MAX){
        println("lidar_doAngle_set: value %d out of range!", state);
        return;
    }

    println("set do angle value: %d .",state);
    lidar.do_angle = state;
}

/*
set error state
param:
    state:error state
*/
void xp_linux_lidar_error_set(int state){
    if(lidar.error != state){
        println("linux lidar error set as: %d .",state);
    }
    if(state==0){
        lidar.error=0;
    }
    else{
        lidar.error=1;
    }
}

/*
get angle point
return: point pointer,point to point data
*/
const LIDAR_angle_point *xp_linux_lidar_anglePoint_get(void){
    return &lidar.msg.angle_point;
}


/*
get lidar park message
return park msg pointer
*/
const LIDAR_park_msg *xp_linux_lidar_park_get(void){
    return &lidar.msg.park;
}


/*
Blowing lidar surface set
雷达表面吹气触发
*/
void xp_linux_lidar_blow(void){
    lidar.blow=lidar.blow_hold_time*1000/LIDAR_blow_cycle_time;
    xp_io_write_pin(1,lidar.blow_io,LIDAR_io_touch);
    println("lidar blow start ~");
}


/*
blowing lidar stop
雷达吹气停止
*/
void xp_linux_lidar_blow_stop(void){
    lidar.blow=0;
    xp_io_write_pin(1,lidar.blow_io,LIDAR_io_release);
    println("lidar blow stop ~");
}

/*
set blow time
param:
    time: set time : s
*/
void xp_linux_lidar_blowTime_set(int time){
    if(time<0){
        time=0-time;
        println("lidar blow set time: %d. reset time= %d.",-time,time);
    }
    lidar.blow_hold_time=time;
    println("lidar blow set hold time: %d.",time);
}


/*
set blow io pin number
param:
    num: pin number 0~23
return:
    0=success
*/
int xp_linux_lidar_blowIO_set(int num){
    if(num<0||num>23){
        perr("lidar blow io set fail ! num= %d.",num);
        lidar.blow_io=100;
        return -1;
    }
    lidar.blow_io=num;
    println("lidar blow io set as: %d.",lidar.blow_io);
    return 0;
}

/*
雷达吹水线程
*/
static void xp_linux_lidar_blow_thread(void *arg){
    println("lidar blow thread create success !");
    lidar.blow_hold_time=LIDAR_blow_time;
    lidar.blow=0;
    while(1){
        if(lidar.blow>0){
            lidar.blow--;
            if(lidar.blow<0){
                lidar.blow=0;
            }
            if(lidar.blow==0){
                xp_linux_lidar_blow_stop();
            }
        }
        aos_msleep(LIDAR_blow_cycle_time);
    }
}

/*
check lidar online status
param:
    lost_deal_fun: lost online deal function
    lost_touch: lost touch deal function time,minutes
*/
void xp_linux_lidar_online_check(void(*lost_deal_fun(void)),int lost_touch){
    static u64 lost_start=0;
    static u8 lost_flag=0;
    u64 now=aos_now_ms();

    if(lidar.msg.data_valid==0){
        if(lost_flag==0){
            lost_start=now;
            lost_flag=1;
        }
        if((now-lost_start)>(lost_touch*60*1000)){
            perr("linux lidar offline over %d minutes,run lost deal function.",lost_touch);
            if(lost_deal_fun!=NULL){
                lost_deal_fun();
            }
            lost_start=now;
        }
        if(lost_flag==1&&(now-lost_start)>5000){
            lidar.lidar_lost=1;
        }
    }
    else{
        lost_flag=0;
        lidar.lidar_lost=0;
    }
}

/*
get lidar lost status
return: lost status,0=normal,1=lost
*/
int xp_linux_lidar_lost_status(void){
    return lidar.lidar_lost;
}



/************************************************debug***********************************/
/*
debug
*/
static int xp_linux_lidar_debug(char *type,char *fun,char *param){
    if(strcmp(type,"linux_lidar")!=0)return 0;
    if(strcmp(fun,"crc")==0){
        u8 crc=xp_crc8((u8 *)param,strlen(param));
        println("%s crc8 = %d",param,crc);
    }
    else if(strcmp(fun,"location_msg")==0){
        xp_linux_lidar_send(param,strlen(param));
    }
    else if(strcmp(fun,"do_state")==0){
        xp_linux_lidar_doAngle_set(atoi(param));
    }
    else if(strcmp(fun,"angle_point_get")==0){
        println("angle point : p1.x=%d,p1.y=%d     p2.x=%d,p2.y=%d   mm",
                                lidar.msg.angle_point.p1.x,
                                lidar.msg.angle_point.p1.y,
                                lidar.msg.angle_point.p2.x,
                                lidar.msg.angle_point.p2.y);
    }
    else if(strcmp(fun,"blow")==0){
        xp_linux_lidar_blow();
    }
    else if(strcmp(fun,"blow_stop")==0){
        xp_linux_lidar_blow_stop();
    }
    else if(strcmp(fun,"blow_time_set")==0){
        xp_linux_lidar_blowTime_set(atoi(param));
    }
    else if(strcmp(fun,"nav")==0){
        lidar.msg.park.parking = atoi(param);
	}
    else if(strcmp(fun,"blow_io")==0){
        println("blow io : %d. init : %d .",lidar.blow_io,lidar.init);
    }
    else{
        return 0;
    }
    return 1;
}


//end of the file
